#include <buola/buola.h>
#include <buola/image/crowsizzler.h>
#include <buola/image/crowbuffer.h>
#include <buola/image/algorithm/stereo.h>
#include <buola/image/algorithm.h>

#include <xmmintrin.h>

namespace buola { namespace img {

void correlation_init_sum(float *pL,float *pR,float *pDst,int pWinSize,int pSizeD,int pSizeX,int /*pW*/,bool pFirst)
{
    int lSumSize=pSizeD*pSizeX;
    int lSumSize_4=lSumSize/4;
    int lWS_2=pWinSize/2;

    pL-=lWS_2;
    pR-=lWS_2;

    if(pFirst)
    {
        __m128 lZero=_mm_setzero_ps();
        for(int i=0;i<lSumSize_4;i++)
            _mm_store_ps(pDst+4*i,lZero);
    }

    for(int d=0;d<pSizeD;d++)
    {
        float lLower=0.0f;
        for(int x=0;x<pWinSize;x++)
            lLower+=std::abs(pL[x]-pR[x+d]);
        pDst[d*pSizeX]+=lLower;
        for(int x=0;x<pSizeX-1;x++)
        {
            lLower+=std::abs(pL[x+pWinSize]-pR[x+d+pWinSize])-
                    std::abs(pL[x]-pR[x+d]);
            pDst[d*pSizeX+x+1]+=lLower;
        }
    }
}

void correlation_init_var(float *pL,float *pDst1,float *pDst2,float *pDst3,int pWinSize,int pSizeX,int /*pW*/,bool pFirst)
{
    float lFac=1.0/(pWinSize*pWinSize);
    int lWS_2=pWinSize/2;

    pL-=lWS_2;

    if(pFirst)
    {
        for(int x=0;x<pSizeX;x++)
        {
            pDst1[x]=0.0;
            pDst2[x]=0.0;
        }
    }

    float lLower=0.0;
    float lLower2=0.0;

    for(int x=0;x<pWinSize;x++)
    {
        lLower+=pL[x];
        lLower2+=pL[x]*pL[x];
    }

    pDst1[0]+=lLower;
    pDst2[0]+=lLower2;

    float lMean=lFac*pDst1[0];
    pDst3[0]=lFac*pDst2[0]-lMean*lMean;

    for(int x=0;x<pSizeX-1;x++)
    {
        lLower+=pL[x+pWinSize]-pL[x];
        lLower2+=pL[x+pWinSize]*pL[x+pWinSize]-pL[x]*pL[x];

        pDst1[x+1]+=lLower;
        pDst2[x+1]+=lLower2;
        lMean=lFac*pDst1[x+1];
        pDst3[x+1]=lFac*pDst2[x+1]-lMean*lMean;
    }
}

void correlation_sum(float *pLNew,float *pLOld,float *pRNew,float *pROld,float *pDst,int pWinSize,
                     int pSizeD,int pSizeX,float *pTmp)
{
    int lWS_2=pWinSize/2;
    pLNew-=lWS_2;
    pLOld-=lWS_2;
    pRNew-=lWS_2;
    pROld-=lWS_2;

    //remove old row
    for(int d=0;d<pSizeD;d++)
        pTmp[d]=0;
    for(int x=0;x<pWinSize;x++)
        for(int d=0;d<pSizeD;d++)
            pTmp[d]+=std::abs(pLOld[x]-pROld[x+d]);
    for(int d=0;d<pSizeD;d++)
        pDst[d*pSizeX]-=pTmp[d];
    for(int x=0;x<pSizeX-1;x++)
    {
        float lLNew=pLOld[x+pWinSize];
        float lLOld=pLOld[x];
        for(int d=0;d<pSizeD;d++)
        {
            pTmp[d]+=std::abs(lLNew-pROld[x+d+pWinSize])-
                     std::abs(lLOld-pROld[x+d]);
            pDst[d*pSizeX+(x+1)]-=pTmp[d];
        }
    }

    //add new row
    for(int d=0;d<pSizeD;d++)
        pTmp[d]=0;
    for(int x=0;x<pWinSize;x++)
        for(int d=0;d<pSizeD;d++)
            pTmp[d]+=std::abs(pLNew[x]-pRNew[x+d]);
    for(int d=0;d<pSizeD;d++)
        pDst[d*pSizeX]+=pTmp[d];
    for(int x=0;x<pSizeX-1;x++)
    {
        float lLNew=pLNew[x+pWinSize];
        float lLOld=pLNew[x];
        for(int d=0;d<pSizeD;d++)
        {
            pTmp[d]+=std::abs(lLNew-pRNew[x+d+pWinSize])-
                     std::abs(lLOld-pRNew[x+d]);
            pDst[d*pSizeX+(x+1)]+=pTmp[d];
        }
    }
}

void correlation_var(float *pLNew,float *pLOld,float *pDst1,float *pDst2,float *pDst3,
                                      int pWinSize,int pSizeX)
{
    float lFac=1.0/(pWinSize*pWinSize);
    int lWS_2=pWinSize/2;

    pLNew-=lWS_2;
    pLOld-=lWS_2;

    float lTmp1=0.0,lTmp2=0.0;

    for(int x=0;x<pWinSize;x++)
    {
        lTmp1+=pLOld[x];
        lTmp2+=pLOld[x]*pLOld[x];
    }
    pDst1[0]-=lTmp1;
    pDst2[0]-=lTmp2;
    for(int x=0;x<pSizeX-1;x++)
    {
        lTmp1+=pLOld[x+pWinSize]-pLOld[x];
        lTmp2+=pLOld[x+pWinSize]*pLOld[x+pWinSize]-pLOld[x]*pLOld[x];
        pDst1[x+1]-=lTmp1;
        pDst2[x+1]-=lTmp2;
    }

    lTmp1=0.0;
    lTmp2=0.0;

    for(int x=0;x<pWinSize;x++)
    {
        lTmp1+=pLNew[x];
        lTmp2+=pLNew[x]*pLNew[x];
    }
    pDst1[0]+=lTmp1;
    pDst2[0]+=lTmp2;
    float lMean=lFac*pDst1[0];
    pDst3[0]=lFac*pDst2[0]-lMean*lMean;
    for(int x=0;x<pSizeX-1;x++)
    {
        lTmp1+=pLNew[x+pWinSize]-pLNew[x];
        lTmp2+=pLNew[x+pWinSize]*pLNew[x+pWinSize]-pLNew[x]*pLNew[x];
        pDst1[x+1]+=lTmp1;
        pDst2[x+1]+=lTmp2;
        float lMean=lFac*pDst1[x+1];
        pDst3[x+1]=lFac*pDst2[x+1]-lMean*lMean;
    }
}

void correlation_max(float *pSrc,float *pDst,int pSizeD,int pSizeX)
{
    //left border
    for(int x=0;x<pSizeD;x++)
    {
        float lMinCorr=1e30;
        float lMinDisp=-1;
        for(int d=0;d<=x;d++)
        {
            if(pSrc[d*pSizeX+(x-d)]<lMinCorr)
            {
                lMinCorr=pSrc[d*pSizeX+(x-d)];
                lMinDisp=d;
            }
        }
        pDst[x]=lMinDisp;
    }

    //center
    for(int x=pSizeD;x<pSizeX;x++)
    {
        float lMinCorr=1e30;
        float lMinDisp=-1;
        for(int d=0;d<pSizeD;d++)
        {
            if(pSrc[d*pSizeX+(x-d)]<lMinCorr)
            {
                lMinCorr=pSrc[d*pSizeX+(x-d)];
                lMinDisp=d;
            }
        }
        pDst[x]=lMinDisp;
    }

    //right border
    for(int x=pSizeX;x<pSizeX+pSizeD;x++)
    {
        float lMinCorr=1e30;
        float lMinDisp=-1;
        for(int d=x-pSizeX+1;d<pSizeD;d++)
        {
            if(pSrc[d*pSizeX+(x-d)]<lMinCorr)
            {
                lMinCorr=pSrc[d*pSizeX+(x-d)];
                lMinDisp=d;
            }
        }
        pDst[x]=lMinDisp;
    }
}

void correlation_find_peak(float *pSrc,float *pDst,float *pRight,float pPThresh,int pSizeD,
                           int pSizeX,float *pVar,float pVThresh)
{
    for(int x=0;x<pSizeX;x++)
    {
        float lMinCorr=1e30;
        int lMinD=0;
        float lSum=0.0;
        for(int d=0;d<pSizeD;d++)
        {
            lSum+=pSrc[d*pSizeX+x];
            if(pSrc[d*pSizeX+x]<lMinCorr)
            {
                lMinCorr=pSrc[d*pSizeX+x];
                lMinD=d;
            }
        }

        if(lMinCorr>pPThresh*lSum||pVar[x]<pVThresh)
            pDst[x]=-2.0;
        else if(pRight[x+lMinD]<lMinD-1||pRight[x+lMinD]>lMinD+1)
            pDst[x]=-1.0;
        else
            pDst[x]=(float)lMinD;
    }
}

void correlation_disparity(const img::CView_gray8 &pLeft,const img::CView_gray8 &pRight,
                           const img::CMView_gray32f &pDisp,int pMin,int pMax,int pWinSize,
                           float pVThresh,bool pLaplace)
{
    const float lPThresh=0.8; //maximum relative peak size
    float lVThresh=pVThresh*256*256;

    int lW=pLeft.Width();
    int lH=pRight.Height();

    int lMinX=-std::min(pMin,0)+pWinSize/2+3;
    int lMaxX=lW-std::max(pMax,0)-pWinSize/2-3;
    int lSizeX=(lMaxX-lMinX)&(~3);
    int lSizeD=pMax-pMin;

    CRowSizzler lSizzler(lW,6,6);
    int lBufW=lSizzler.GetTotalW();

    CRowBuffer<float> lLSizBuf(lBufW,1);
    CRowBuffer<float> lLLPXBuf(lBufW,1);
    CRowBuffer<float> lLLPYBuf(lBufW,7);
    CRowBuffer<float> lLLapBuf(lBufW,1);
    CRowBuffer<float> lLDeBuf(lW,pWinSize+1);
    CRowBuffer<float> lRSizBuf(lBufW,1);
    CRowBuffer<float> lRLPXBuf(lBufW,1);
    CRowBuffer<float> lRLPYBuf(lBufW,7);
    CRowBuffer<float> lRLapBuf(lBufW,1);
    CRowBuffer<float> lRDeBuf(lW,pWinSize+1);
    CRowBuffer<float> lSumBuf(lSizeX*lSizeD,1);
    CRowBuffer<float> lVarBuf(lSizeX,3);
    CRowBuffer<float> lTmpBuf(4*4*lSizeD,1);
    CRowBuffer<float> lRigBuf(lSizeX+lSizeD,1);

    const unsigned char *lLeft=pLeft.RawData();
    const unsigned char *lRight=pRight.RawData();
    float *lDisp=pDisp.RawData();

    for(int y=0;y<lH;y++)
    {
        lSizzler.Sizzle(lLeft+y*lW,lLSizBuf[y]);
        lSizzler.Sizzle(lRight+y*lW,lRSizBuf[y]);

        sizzle_lowpass_x(lLSizBuf[y],lLLPXBuf[y],lSizzler);
        sizzle_lowpass_x(lRSizBuf[y],lRLPXBuf[y],lSizzler);

        if (y==0)
        {
            sizzle_first_lowpass_y(lLLPXBuf[y],lLLPYBuf[y],lLLPYBuf[y+1],
                lLLPYBuf[y+2],lLLPYBuf[y+3],lLLPYBuf[y+4],lSizzler);
            sizzle_first_lowpass_y(lRLPXBuf[y],lRLPYBuf[y],lRLPYBuf[y+1],
                lRLPYBuf[y+2],lRLPYBuf[y+3],lRLPYBuf[y+4],lSizzler);
        }
        else
        {
            sizzle_lowpass_y(lLLPXBuf[y],lLLPYBuf[y],lLLPYBuf[y+1],
                lLLPYBuf[y+2],lLLPYBuf[y+3],lLLPYBuf[y+4],lSizzler);
            sizzle_lowpass_y(lRLPXBuf[y],lRLPYBuf[y],lRLPYBuf[y+1],
                lRLPYBuf[y+2],lRLPYBuf[y+3],lRLPYBuf[y+4],lSizzler);
        }

        if(y<2) continue;

        if(pLaplace)
        {
            sizzle_laplace(lLLPYBuf[y-2],lLLPYBuf[y-1],lLLPYBuf[y],lLLapBuf[y],lSizzler);
            sizzle_laplace(lRLPYBuf[y-2],lRLPYBuf[y-1],lRLPYBuf[y],lRLapBuf[y],lSizzler);
            lSizzler.DeSizzle(lLLapBuf[y],lLDeBuf[y]);
            lSizzler.DeSizzle(lRLapBuf[y],lRDeBuf[y]);
        }
        else
        {
            lSizzler.DeSizzle(lLLPYBuf[y],lLDeBuf[y]);
            lSizzler.DeSizzle(lRLPYBuf[y],lRDeBuf[y]);
        }

        if(y<2+pWinSize)
        {
            correlation_init_sum(lLDeBuf[y]+lMinX,lRDeBuf[y]+lMinX+pMin,lSumBuf[0],pWinSize,lSizeD,lSizeX,lW,y==2);
            correlation_init_var(lLDeBuf[y]+lMinX,lVarBuf[0],lVarBuf[1],lVarBuf[2],pWinSize,lSizeX,lW,y==2);
        }
        else
        {
            correlation_sum(lLDeBuf[y]+lMinX,lLDeBuf[y-pWinSize]+lMinX,
                            lRDeBuf[y]+lMinX+pMin,lRDeBuf[y-pWinSize]+lMinX+pMin,
                            lSumBuf[0],pWinSize,lSizeD,lSizeX,lTmpBuf[0]);
            correlation_var(lLDeBuf[y]+lMinX,lLDeBuf[y-pWinSize]+lMinX,
                            lVarBuf[0],lVarBuf[1],lVarBuf[2],pWinSize,lSizeX);
        }

        if(y>=1+pWinSize)
        {
            correlation_max(lSumBuf[0],lRigBuf[0],lSizeD,lSizeX);
            correlation_find_peak(lSumBuf[0],lDisp+lW*(y-(1+pWinSize)/2)+lMinX,lRigBuf[0],
                                  lPThresh/lSizeD,lSizeD,lSizeX,lVarBuf[2],lVThresh);
        }
    }
}

inline unsigned char dp_dist(unsigned char pL,unsigned char pR)
{
    return (pL>pR)?(pL-pR):(pR-pL);
}

inline float dp_dist(float pL,float pR)
{
    return (pL>pR)?(pL-pR):(pR-pL);
}

inline unsigned char dp_min(int p0,int p1,int p2,int &pDst)
{
    if(p0<=p1)
    {
        if(p0<=p2)
        {
            pDst = p0;
            return 0;
        }
        else
        {
            pDst = p2;
            return 2;
        }
    }
    else
    {
        if(p1<=p2)
        {
            pDst = p1;
            return 1;
        }
        else
        {
            pDst = p2;
            return 2;
        }
    }
}

inline unsigned char dp_min(float p0,float p1,float p2,float &pDst)
{
    if(p0<=p1)
    {
        if(p0<=p2)
        {
            pDst = p0;
            return 0;
        }
        else
        {
            pDst = p2;
            return 2;
        }
    }
    else
    {
        if(p1<=p2)
        {
            pDst = p1;
            return 1;
        }
        else
        {
            pDst = p2;
            return 2;
        }
    }
}

void dp_match_lines(unsigned char *pL,unsigned char *pR,float *pD,int pW)
{
    CRowBuffer<unsigned char> lDist(pW,pW);
    CRowBuffer<int> lAccDist(pW,pW);
    CRowBuffer<unsigned char> lMovements(pW,pW);

    int l,r;
    // Distance matrix loop
    for(l=0;l<pW;l++)
    {
        for(r=0;r<pW;r++)
        {
            lDist[l][r]=0;
            for(int x=-7;x<=7;x++)
                for(int y=-7;y<=7;y++)
                    lDist[l][r]+=dp_dist(pL[l+x+y*pW],pR[r+x+y*pW]);
            lAccDist[l][r]=(int)lDist[l][r];
        }
    }
    // Accumulating distance matrix loop
    for(l=1;l<pW;l++)
    {
        for(r=1;r<pW;r++)
        {
            int lMin;
            lMovements[l][r] = dp_min(lAccDist[l][r-1],lAccDist[l-1][r],lAccDist[l-1][r-1],lMin);
            lAccDist[l][r] += (int)lMin;
        }
    }
    // Backtrace loop: store the best index in pRRow for pLRow
    for(l=pW-1,r=pW-1;l>0 && r>0;) // decremented inside
    {
        pD[l] = l-r; // will be overwritten if l is not decremented, but it's OK
        switch (lMovements[l][r])
        {
            case 0:
                r--; break; // move just in r;
            case 1:
                l--;break; // move just in l;
            case 2:
                r--;l--; break; // move both in l and r;
            default:
                msg_info() << "Erroneous movement stored in lMovements[" << l << "][" << r << "]\n";
                exit(-1);
        }
    }
    // if pLRow[0] doesn't have a match in pRRow, tipically the first part of pLRow will be matched
    // to pRRow[0], until a pixel with a proper match appears. In that case, we haven't assigned yet
    // any matches to the first pixels in pDRow: let's assign them to 0!
    while(l>0)
    {
        pD[l] = l;
        l--;
    }
}

void dp_match_lines(float *pL,float *pR,float *pD,int pW)
{
    CRowBuffer<float> lDist(pW,pW);
    CRowBuffer<float> lAccDist(pW,pW);
    CRowBuffer<unsigned char> lMovements(pW,pW);

    int l,r;
    // Distance matrix loop
    for(l=0;l<pW;l++)
    {
        for(r=0;r<pW;r++)
        {
            lDist[l][r]=0;
            for(int x=0;x<=0;x++)
                for(int y=0;y<=0;y++)
                    lDist[l][r]+=dp_dist(pL[std::min(pW-1,std::max(0,l+x))+y*pW],pR[std::min(pW-1,std::max(0,r+x))+y*pW]);
            lAccDist[l][r]=lDist[l][r];
        }
    }
    // Accumulating distance matrix loop
    for(l=1;l<pW;l++)
    {
        for(r=1;r<pW;r++)
        {
            float lMin;
            lMovements[l][r] = dp_min(lAccDist[l][r-1],lAccDist[l-1][r],lAccDist[l-1][r-1],lMin);
            lAccDist[l][r] += lMin;
        }
    }
    // Backtrace loop: store the best index in pRRow for pLRow
    for(l=pW-1,r=pW-1;l>0 && r>0;) // decremented inside
    {
        pD[l] = l-r; // will be overwritten if l is not decremented, but it's OK
        switch (lMovements[l][r])
        {
            case 0:
                r--; break; // move just in r;
            case 1:
                l--;break; // move just in l;
            case 2:
                r--;l--; break; // move both in l and r;
            default:
                msg_info() << "Erroneous movement stored in lMovements[" << l << "][" << r << "]\n";
                exit(-1);
        }
    }
    // if pLRow[0] doesn't have a match in pRRow, tipically the first part of pLRow will be matched
    // to pRRow[0], until a pixel with a proper match appears. In that case, we haven't assigned yet
    // any matches to the first pixels in pDRow: let's assign them to 0!
    while(l>0)
    {
        pD[l] = l;
        l--;
    }
}

void dp_disparity(const img::CView_gray8 &pLeft,const img::CView_gray8 &pRight,
                  const img::CMView_gray32f &pDisp,int pWinSize,bool /*pLaplace*/)
{
    int lW=pLeft.Width();
    int lH=pRight.Height();

//    int lMinX=-std::min(pMin,0)+pWinSize/2+3;
//    int lMaxX=lW-std::max(pMax,0)-pWinSize/2-3;
//    int lSizeX=(lMaxX-lMinX)&(~3);
//    int lSizeD=pMax-pMin;

    CRowSizzler lSizzler(lW,6,6);
    int lBufW=lSizzler.GetTotalW();

    CRowBuffer<float> lLSizBuf(lBufW,1);
    CRowBuffer<float> lLLPXBuf(lBufW,1);
    CRowBuffer<float> lLLPYBuf(lBufW,7);
    CRowBuffer<float> lLLapBuf(lBufW,1);
    CRowBuffer<float> lLDeBuf(lW,pWinSize+1+lH);
    CRowBuffer<float> lRSizBuf(lBufW,1);
    CRowBuffer<float> lRLPXBuf(lBufW,1);
    CRowBuffer<float> lRLPYBuf(lBufW,7);
    CRowBuffer<float> lRLapBuf(lBufW,1);
    CRowBuffer<float> lRDeBuf(lW,pWinSize+1+lH);
//    CRowBuffer<float> lSumBuf(lSizeX*lSizeD,1);
//    CRowBuffer<float> lVarBuf(lSizeX,3);
//    CRowBuffer<float> lTmpBuf(4*4*lSizeD,1);
//    CRowBuffer<float> lRigBuf(lSizeX+lSizeD,1);

    const unsigned char *lLeft=pLeft.RawData();
    const unsigned char *lRight=pRight.RawData();
    float *lDisp=pDisp.RawData();

    for(int y=0;y<lH;y++)
    {
        lSizzler.Sizzle(lLeft+y*lW,lLSizBuf[y]);
        lSizzler.Sizzle(lRight+y*lW,lRSizBuf[y]);

        sizzle_lowpass_x(lLSizBuf[y],lLLPXBuf[y],lSizzler);
        sizzle_lowpass_x(lRSizBuf[y],lRLPXBuf[y],lSizzler);

        if (y==0)
        {
            sizzle_first_lowpass_y(lLLPXBuf[y],lLLPYBuf[y],lLLPYBuf[y+1],
                lLLPYBuf[y+2],lLLPYBuf[y+3],lLLPYBuf[y+4],lSizzler);
            sizzle_first_lowpass_y(lRLPXBuf[y],lRLPYBuf[y],lRLPYBuf[y+1],
                lRLPYBuf[y+2],lRLPYBuf[y+3],lRLPYBuf[y+4],lSizzler);
        }
        else
        {
            sizzle_lowpass_y(lLLPXBuf[y],lLLPYBuf[y],lLLPYBuf[y+1],
                lLLPYBuf[y+2],lLLPYBuf[y+3],lLLPYBuf[y+4],lSizzler);
            sizzle_lowpass_y(lRLPXBuf[y],lRLPYBuf[y],lRLPYBuf[y+1],
                lRLPYBuf[y+2],lRLPYBuf[y+3],lRLPYBuf[y+4],lSizzler);
        }

        if(y<2) continue;

        if(0)//pLaplace)
        {
            sizzle_laplace(lLLPYBuf[y-2],lLLPYBuf[y-1],lLLPYBuf[y],lLLapBuf[y],lSizzler);
            sizzle_laplace(lRLPYBuf[y-2],lRLPYBuf[y-1],lRLPYBuf[y],lRLapBuf[y],lSizzler);
            lSizzler.DeSizzle(lLLapBuf[y],lLDeBuf[y]);
            lSizzler.DeSizzle(lRLapBuf[y],lRDeBuf[y]);
        }
        else
        {
//            lSizzler.DeSizzle(lLLPYBuf[y],lLDeBuf[y]);
//            lSizzler.DeSizzle(lRLPYBuf[y],lRDeBuf[y]);
            lSizzler.DeSizzle(lLSizBuf[y],lLDeBuf[y]);
            lSizzler.DeSizzle(lRSizBuf[y],lRDeBuf[y]);
        }

        if(y>=15)
        dp_match_lines(lLDeBuf[y],lRDeBuf[y],lDisp+lW*y,lW);
/*
        if(y<2+pWinSize)
        {
            correlation_init_sum(lLDeBuf[y]+lMinX,lRDeBuf[y]+lMinX+pMin,lSumBuf[0],pWinSize,lSizeD,lSizeX,lW,y==2);
            correlation_init_var(lLDeBuf[y]+lMinX,lVarBuf[0],lVarBuf[1],lVarBuf[2],pWinSize,lSizeX,lW,y==2);
        }
        else
        {
            correlation_sum(lLDeBuf[y]+lMinX,lLDeBuf[y-pWinSize]+lMinX,
                            lRDeBuf[y]+lMinX+pMin,lRDeBuf[y-pWinSize]+lMinX+pMin,
                            lSumBuf[0],pWinSize,lSizeD,lSizeX,lTmpBuf[0]);
            correlation_var(lLDeBuf[y]+lMinX,lLDeBuf[y-pWinSize]+lMinX,
                            lVarBuf[0],lVarBuf[1],lVarBuf[2],pWinSize,lSizeX);
        }

        if(y>=1+pWinSize)
        {
            correlation_max(lSumBuf[0],lRigBuf[0],lSizeD,lSizeX);
            correlation_find_peak(lSumBuf[0],lDisp+lW*(y-(1+pWinSize)/2)+lMinX,lRigBuf[0],
                                  lPThresh/lSizeD,lSizeD,lSizeX,lVarBuf[2],lVThresh);
        }
*/
    }
    for(int y=0;y<pLeft.Height();y++)
    {
    }
}

void dp_disparity(const img::CView_rgb8 &pLeft,const img::CView_rgb8 &pRight,
                  const img::CMView_gray32f &pDisp,int pWinSize,bool /*pLaplace*/)
{
    int lW=pLeft.Width();
    int lH=pRight.Height();

//    int lMinX=-std::min(pMin,0)+pWinSize/2+3;
//    int lMaxX=lW-std::max(pMax,0)-pWinSize/2-3;
//    int lSizeX=(lMaxX-lMinX)&(~3);
//    int lSizeD=pMax-pMin;

    CRowSizzler lSizzler(lW,6,6);
    int lBufW=lSizzler.GetTotalW();

    CRowBuffer<float> lLSizBuf(lBufW,1);
    CRowBuffer<float> lLLPXBuf(lBufW,1);
    CRowBuffer<float> lLLPYBuf(lBufW,7);
    CRowBuffer<float> lLLapBuf(lBufW,1);
    CRowBuffer<float> lLDeBuf(lW,pWinSize+1+lH);
    CRowBuffer<float> lRSizBuf(lBufW,1);
    CRowBuffer<float> lRLPXBuf(lBufW,1);
    CRowBuffer<float> lRLPYBuf(lBufW,7);
    CRowBuffer<float> lRLapBuf(lBufW,1);
    CRowBuffer<float> lRDeBuf(lW,pWinSize+1+lH);
//    CRowBuffer<float> lSumBuf(lSizeX*lSizeD,1);
//    CRowBuffer<float> lVarBuf(lSizeX,3);
//    CRowBuffer<float> lTmpBuf(4*4*lSizeD,1);
//    CRowBuffer<float> lRigBuf(lSizeX+lSizeD,1);

    const unsigned char *lLeft=pLeft.RawData();
    const unsigned char *lRight=pRight.RawData();
    float *lDisp=pDisp.RawData();

    for(int y=0;y<lH;y++)
    {
        lSizzler.Sizzle(lLeft+y*lW,lLSizBuf[y]);
        lSizzler.Sizzle(lRight+y*lW,lRSizBuf[y]);

        sizzle_lowpass_x(lLSizBuf[y],lLLPXBuf[y],lSizzler);
        sizzle_lowpass_x(lRSizBuf[y],lRLPXBuf[y],lSizzler);

        if (y==0)
        {
            sizzle_first_lowpass_y(lLLPXBuf[y],lLLPYBuf[y],lLLPYBuf[y+1],
                lLLPYBuf[y+2],lLLPYBuf[y+3],lLLPYBuf[y+4],lSizzler);
            sizzle_first_lowpass_y(lRLPXBuf[y],lRLPYBuf[y],lRLPYBuf[y+1],
                lRLPYBuf[y+2],lRLPYBuf[y+3],lRLPYBuf[y+4],lSizzler);
        }
        else
        {
            sizzle_lowpass_y(lLLPXBuf[y],lLLPYBuf[y],lLLPYBuf[y+1],
                lLLPYBuf[y+2],lLLPYBuf[y+3],lLLPYBuf[y+4],lSizzler);
            sizzle_lowpass_y(lRLPXBuf[y],lRLPYBuf[y],lRLPYBuf[y+1],
                lRLPYBuf[y+2],lRLPYBuf[y+3],lRLPYBuf[y+4],lSizzler);
        }

        if(y<2) continue;

        if(0)//pLaplace)
        {
            sizzle_laplace(lLLPYBuf[y-2],lLLPYBuf[y-1],lLLPYBuf[y],lLLapBuf[y],lSizzler);
            sizzle_laplace(lRLPYBuf[y-2],lRLPYBuf[y-1],lRLPYBuf[y],lRLapBuf[y],lSizzler);
            lSizzler.DeSizzle(lLLapBuf[y],lLDeBuf[y]);
            lSizzler.DeSizzle(lRLapBuf[y],lRDeBuf[y]);
        }
        else
        {
//            lSizzler.DeSizzle(lLLPYBuf[y],lLDeBuf[y]);
//            lSizzler.DeSizzle(lRLPYBuf[y],lRDeBuf[y]);
            lSizzler.DeSizzle(lLSizBuf[y],lLDeBuf[y]);
            lSizzler.DeSizzle(lRSizBuf[y],lRDeBuf[y]);
        }

        if(y>=15)
        dp_match_lines(lLDeBuf[y],lRDeBuf[y],lDisp+lW*y,lW);
/*
        if(y<2+pWinSize)
        {
            correlation_init_sum(lLDeBuf[y]+lMinX,lRDeBuf[y]+lMinX+pMin,lSumBuf[0],pWinSize,lSizeD,lSizeX,lW,y==2);
            correlation_init_var(lLDeBuf[y]+lMinX,lVarBuf[0],lVarBuf[1],lVarBuf[2],pWinSize,lSizeX,lW,y==2);
        }
        else
        {
            correlation_sum(lLDeBuf[y]+lMinX,lLDeBuf[y-pWinSize]+lMinX,
                            lRDeBuf[y]+lMinX+pMin,lRDeBuf[y-pWinSize]+lMinX+pMin,
                            lSumBuf[0],pWinSize,lSizeD,lSizeX,lTmpBuf[0]);
            correlation_var(lLDeBuf[y]+lMinX,lLDeBuf[y-pWinSize]+lMinX,
                            lVarBuf[0],lVarBuf[1],lVarBuf[2],pWinSize,lSizeX);
        }

        if(y>=1+pWinSize)
        {
            correlation_max(lSumBuf[0],lRigBuf[0],lSizeD,lSizeX);
            correlation_find_peak(lSumBuf[0],lDisp+lW*(y-(1+pWinSize)/2)+lMinX,lRigBuf[0],
                                  lPThresh/lSizeD,lSizeD,lSizeX,lVarBuf[2],lVThresh);
        }
*/
    }
    for(int y=0;y<pLeft.Height();y++)
    {
    }
}

/*namespace cvi*/ } /*namespace buola*/ }
